from enum import Enum, unique
import math
from .motor_math import StepperMotorMath

@unique
class TrapeziodStatus(Enum):
    """
    梯形加减速状态

    加速、匀速、减速
    """
    ACC = 0     #加速
    CON = 1     #匀速
    DEC = 2     #减速

class TrapezoidMotion:
    """
    步进电机梯形加减速

    步进电机梯形加减速
    """

    def __init__(self) -> None:
        pass

    def init(self, acc_rpss:float, dec_rpss:float, start_v_rpm:int, max_v_rpm:int, end_v_rpm:int, step_num:int, spr:int, timer_freq:int)->None:
        self.__acc_rpss:float = acc_rpss                                              #加速度,rad/s2(rpss)
        self.__dec__rpss:float = dec_rpss                                             #减速度,rad/s2(rpss)
        self.__start_v_rps:float = StepperMotorMath.rpm_2_rps(start_v_rpm)          #起始速度,rad/s(RPS)
        self.__max_v_rps:float = StepperMotorMath.rpm_2_rps(max_v_rpm)              #最高转速,rad/s(RPS)
        self.__end_v_rps:float = StepperMotorMath.rpm_2_rps(end_v_rpm)              #终点速度,rad/s(RPS)
        self.__step_num = step_num                                                  #总运行步数
        self.__spr = spr                                                            #电机每圈的步数（细分步数）
        self.__th:float = 2*math.pi/self.__spr                                      #θ,步进电机每一步（细分步）对应的弧度
        self.__timer_freq = timer_freq                                              #定时器频率
        self.__cur_step:int = 0                                                     #当前步数
        self.__last_cn:int = 0                                                      #上一步定时器计数值cn
        self.__dec_step_num:int = 0                                                 #减速起始步数
        self.__status:TrapeziodStatus = TrapeziodStatus.ACC                         #默认为加速状态

        #完成完整的加减速过程所需的最小步数
        self.__min_step:int = math.ceil(self.__calcu_min_step(self.__acc_rpss, self.__dec__rpss, self.__start_v_rps, self.__max_v_rps, self.__end_v_rps, self.__th))

        #总步数足够电机完成完整的加减速过程
        if(self.__step_num > self.__min_step):
            self.__is_trap_complete:bool = True
        #在总行程中无法完成完成的加减速过程，即电机需要提前减速
        else:
            self.__is_trap_complete = False  
            self.__max_v_rps = math.sqrt((2*self.__acc_rpss*self.__dec__rpss*self.__step_num*self.__th + self.__dec__rpss*self.__start_v_rps*self.__start_v_rps + \
                                      self.__acc_rpss*self.__end_v_rps*self.__end_v_rps)/(self.__acc_rpss + self.__dec__rpss)) 
             
        #计算最高转速对应的cn
        self.__min_cn = StepperMotorMath.rps_2_cn(self.__max_v_rps, self.__timer_freq, self.__spr) 
        
        #计算减速所需步数
        self.__dec_step_num = math.ceil((self.__max_v_rps*self.__max_v_rps-self.__end_v_rps*self.__end_v_rps)/\
                                  (2*self.__dec__rpss*self.__th))
    
    
    def __calcu_cn(self, freq:int, acc_dec:float, th:float, start_end_v:float, n:int)->int:
        cn:int = freq*(math.sqrt(2*acc_dec*(n+1)*th+start_end_v*start_end_v)-math.sqrt(2*acc_dec*n*th+start_end_v*start_end_v))/acc_dec
        return cn


    def __calcu_min_step(self, acc:float, dec:float, start_v:float, max_v:float, end_v:float, th:float)->int:
        """
        计算完成完整的加减速所需的最小步数

        Parameters
        ----------
        acc : int,加速度

        dec : int,减速度  

        start_v : int,起始速度

        max_v : int,最大转速

        end_v : int,停止速度
        
        Returns
        -------
        min_step : int,完成完整的加减速所需的最小步数
        """
        return (((acc+dec)*max_v*max_v)-(dec*start_v*start_v)-(acc*end_v*end_v))/(2*acc*dec*th)
    

    def __calcu_next_cn_is_trap_complete(self, is_trap_complete:bool)->int:
        cn:int = self.__min_cn
        #加速阶段
        if self.__status == TrapeziodStatus.ACC:
            #计算下一cn
            cn:int = self.__calcu_cn(self.__timer_freq, self.__acc_rpss, self.__th, self.__start_v_rps, self.__cur_step)
            #达到最高转速，进入匀速阶段或者减速阶段，取决于，能否完成完整的加减速过程
            if cn <= self.__min_cn:
                cn = self.__min_cn
                if is_trap_complete:
                    self.__status = TrapeziodStatus.CON
                else:
                    self.__status = TrapeziodStatus.DEC
            self.__cur_step += 1
        #匀阶段    
        elif self.__status == TrapeziodStatus.CON:
            self.__cur_step += 1
            #到达减速阶段
            if self.__cur_step >= self.__step_num - self.__dec_step_num:
                self.__status = TrapeziodStatus.DEC
        #减速阶段
        else:
            cn = self.__calcu_cn(self.__timer_freq, self.__dec__rpss, self.__th, self.__end_v_rps, self.__step_num - self.__cur_step - 1)
            self.__cur_step += 1
        return cn
            
    def calcu_next_cn(self)->int:
        if self.__cur_step >= self.__step_num:
            return self.__last_cn
        else:
            self.__last_cn = self.__calcu_next_cn_is_trap_complete(self.__is_trap_complete)
            return self.__last_cn
    